#include "pointcloudwidget.h"
#include "node/rosnode.h"
#include "toast.h"

#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include "tools/leiliconfig.h"
#include "util/rosbag.h"




PointCloudWidget::PointCloudWidget(QWidget *parent) : QWidget(parent)
{
    setupUI();


    GRosNode->subPoint2(QString::fromStdString(TopicFiled::point2Save));
    QObject::connect(m_btnSave,&QPushButton::clicked,this,&PointCloudWidget::onSave);
}

void PointCloudWidget::onSave()
{
    auto msg = GRosNode->getPoint2Data(QString::fromStdString(TopicFiled::point2Save));
    if(msg == nullptr)
    {
        Toast::toast("warningImg", "尚未接收到点云数据", QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));
        return;
    }


    if(0){
        // 转换ROS消息为PCL点云
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);

        pcl::moveFromROSMsg(*msg, *cloud);
        //pcl::fromROSMsg(*msg, *cloud);

        // 保存为PCD文件（ASCII格式）
        std::string filename =LeiliCfg->getSavePath().toStdString()  + "/" + "output.pcd";


        pcl::io::savePCDFileASCII(filename, *cloud);
    }



    std::string filename =LeiliCfg->getSavePath().toStdString()  + "/" + "output.bag";

    msg->header.frame_id = "map";
    bool succ = RosBagSaver::writePoint2(filename,*msg);
    if(!succ)
    {
        Toast::toast("warningImg", "保存失败", QPoint(this->x() + this->width() / 2, this->y() + this->height() / 2));
        return;
    }

    Toast::toast("succImg","保存成功",QPoint(this->pos().x() + this->width()/2,this->pos().y() + this->height()/2),nullptr);
}


void PointCloudWidget::setupUI()
{
    QVBoxLayout *vl =new QVBoxLayout(this);
    vl->setContentsMargins(0,0,0,0);

    m_rviz = new QRviz(this);
    m_rviz->Display_Grid(true, "<Fixed Frame>",QColor(160,160,164));
    m_rviz->Display_Axes(true,"<Fixed Frame>","2","0.3");


    QHBoxLayout *hB = new QHBoxLayout(this);
    m_btnOpenLader = new QPushButton(this);
    m_btnOpenLader->setText("打开");

    m_btnSave = new QPushButton(this);
    m_btnSave->setText("保存点云");

    m_btnOpenRogBag = new QPushButton(this);
    m_btnOpenRogBag->setText("查看");

    hB->addStretch();
    hB->addWidget(m_btnOpenLader);
    hB->addWidget(m_btnSave);
    hB->addWidget(m_btnOpenRogBag);
    hB->addStretch();

    vl->addWidget(m_rviz);
    vl->addLayout(hB);

}
